#pragma once

#include "NavParameterCalculate.hpp"

namespace ns_Calculate
{
    class NavCalculateResult : public NavParameterCalculate
    {
    public:
        NavCalculateResult(const GNSSMesData::Ptr& input,const TimeFactory::TimePtr& curTime)
            : NavParameterCalculate(input, curTime)
        {}

        // 在地心地固坐标系下的坐标
        CRDFactory::CRDPtr GetPosition()
        {
            double _xk = _parameter.Rk * Mathf::Cos(_parameter.Uk);
            double _yk = _parameter.Rk * Mathf::Sin(_parameter.Uk);

            double x = _xk * Mathf::Cos(_parameter.OMGk) - _yk * Mathf::Cos(_parameter.Ik) * Mathf::Sin(_parameter.OMGk);
            double y = _xk * Mathf::Sin(_parameter.OMGk) + _yk * Mathf::Cos(_parameter.Ik) * Mathf::Cos(_parameter.OMGk);
            double z = _yk * Mathf::Sin(_parameter.Ik);

            if (_input->GetGNSSType() == GNSSType::BDS && _input->GetRecord()->serial <= 5)
            {
                auto inputRecord = _input->GetRecord();
                double tmpx = Mathf::Cos(Mathf::omgeBDS * _parameter.tk) * x + Mathf::Sin(Mathf::omgeBDS * _parameter.tk) * Mathf::Cos(-5 * Mathf::Pi / 180) * y + Mathf::Sin(Mathf::omgeBDS * _parameter.tk) * Mathf::Sin(-5 * Mathf::Pi / 180) * z;
                double tmpy = -Mathf::Sin(Mathf::omgeBDS * _parameter.tk) * x + Mathf::Cos(Mathf::omgeBDS * _parameter.tk) * Mathf::Cos(-5 * Mathf::Pi / 180) * y + Mathf::Cos(Mathf::omgeBDS * _parameter.tk) * Mathf::Sin(-5 * Mathf::Pi / 180) * z;
                double tmpz = -Mathf::Sin(-5 * Mathf::Pi / 180) * y + Mathf::Cos(-5 * Mathf::Pi / 180) * z;

                x = tmpx;
                y = tmpy;
                z = tmpz;
            }

            return CRDFactory::MakeCRDPtr(CRDFactory::ValueType::CartesianCRDSys,x,y,z);
        }

        std::vector<double> GetVelocity()
        {
            auto inputHead = _input->GetHeader();
            auto inputRecord = _input->GetRecord();

            double xk = _parameter.Rk * Mathf::Cos(_parameter.Uk);
            double yk = _parameter.Rk * Mathf::Sin(_parameter.Uk);

            double Ek = _parameter.angularVelocity / (1 - inputRecord->e * Mathf::Cos(_parameter.Ek));

            double Vk = Mathf::Sqrt((1 + inputRecord->e) / (1 - inputRecord->e)) *
                        Mathf::Pow((Mathf::Cos(_parameter.Vk / 2) / Mathf::Cos(_parameter.Ek / 2)), 2) *
                        Ek;

            double Uk = Vk * (1 + 2 * inputRecord->cus * Mathf::Cos(2 * _parameter.Vk) - 2 * inputRecord->cuc * Mathf::Sin(2 * _parameter.Vk));
            double Rk = Ek * _parameter.sqrtA * _parameter.sqrtA * Mathf::Sin(_parameter.Vk) + 2 * Vk * (inputRecord->crs * Mathf::Cos(2 * Vk) - inputRecord->crc * Mathf::Sin(2 * Vk));
            double Ik = inputRecord->IDOT + 2 * Vk * (inputRecord->cis * Mathf::Cos(2 * _parameter.Vk) - inputRecord->cic * Mathf::Sin(2 * _parameter.Vk));

            double Xk = Rk * Mathf::Cos(_parameter.Uk) - _parameter.Rk * Uk * Mathf::Sin(_parameter.Uk);
            double Yk = Rk * Mathf::Sin(_parameter.Uk) + _parameter.Rk * Uk * Mathf::Cos(_parameter.Uk);

            double Omgk = inputRecord->deltaOmega - Mathf::omgeGPS;
            if (_gnssType = GNSSType::BDS)
            {
                Omgk = inputRecord->deltaOmega;
            }

            std::vector<double> ret(3);
            ret[0] = Xk * Mathf::Cos(_parameter.OMGk) - Yk * Mathf::Cos(_parameter.Ik) * Mathf::Sin(_parameter.OMGk) - Omgk * (xk * Mathf::Sin(_parameter.OMGk) + yk * Mathf::Cos(_parameter.Ik) * Mathf::Cos(_parameter.OMGk)) + Ik * yk * Mathf::Sin(_parameter.Ik) * Mathf::Sin(_parameter.OMGk);
            ret[1] = Xk * Mathf::Sin(_parameter.OMGk) + Yk * Mathf::Cos(_parameter.Ik) * Mathf::Cos(_parameter.OMGk) + Omgk * (xk * Mathf::Cos(_parameter.OMGk) - yk * Mathf::Cos(_parameter.Ik) * Mathf::Sin(_parameter.OMGk)) - Ik * yk * Mathf::Sin(_parameter.Ik) * Mathf::Sin(_parameter.OMGk);
            ret[2] = Yk * Mathf::Sin(_parameter.Ik) + Ik * yk * Mathf::Cos(_parameter.Ik);

            return ret;
        }

        double GetClockBias()
        {
            auto inputHead = _input->GetHeader();
            auto inputRecord = _input->GetRecord();

            auto bt = TimeFactory::MakeTimePtr(TimeFactory::ValueType::GPSTimeSys,inputRecord->GPSWeek,inputRecord->TOE);
            double timeGap = _curTime->SecondFromRefTime() - bt->SecondFromRefTime();

            double deltaTr = -4.442807633E-10 * inputRecord->e * _parameter.sqrtA * Mathf::Sin(_parameter.Ek);
            double deltaTsv = inputRecord->a0 + inputRecord->a1 * (timeGap) + inputRecord->a2 * timeGap * timeGap + deltaTr;

            return deltaTsv;
        }

        double GetClockSpeed()
        {
            auto inputHead = _input->GetHeader();
            auto inputRecord = _input->GetRecord();

            double Ek = _parameter.angularVelocity / (1 - inputRecord->e * Mathf::Cos(_parameter.Ek));

            auto bt = TimeFactory::MakeTimePtr(TimeFactory::ValueType::GPSTimeSys,inputRecord->GPSWeek,inputRecord->TOE);
            double timeGap = _curTime->SecondFromRefTime() - bt->SecondFromRefTime();

            double deltaTr = -4.442807633E-10 * inputRecord->e * _parameter.sqrtA * Mathf::Cos(_parameter.Ek) * Ek;
            double deltaTsv = inputRecord->a1 + inputRecord->a2 * timeGap + deltaTr;

            return deltaTsv;
        }
    };
}